#include "../includes.h"

int prev_running_time, prev_power_time;
/* OCXO Control */
CONTROL_    CONTROL;


/* Locked */
int ControlCounter;

/* DAC */
double DeltaDAC;


int		QuickStartCounter;
int		FIRST_LOCK, FIRST_STANDBY;
char	LOCK_REFERENCE;
int		bDacUpdated = FALSE;
double	average_rd_now = 0.0;
double	average_rd_last = 0.0;
int		average_rd_plus = 0;
int		average_rd_minus = 0;

/* --------------------- */
/* Kalman Filter         */
/* --------------------- */
Matrics GainK, HatX, HatXm0, Pm0, P0, PhiK, QK, HK, IK, U;
Matrics R;
double 	RK;
float 	KGainF,DumyKGainF;
int 	KInterval,KNextInterval;
float 	Kp,Ki,T;
double 	GPSPrevU,GPSCurrentU;

float 	Modified_RAW_OFFSET;

#define MAX_DACTable_SIZE        20

int 	DACTableIndex;
double 	MeanDAC,MeanU;
double 	DACTable[MAX_DACTable_SIZE],UTable[MAX_DACTable_SIZE];
int 	Tracking_Interval_Counter;


FreqFilter ffD;
int sync_by_ddac;

char control_mode = 0;
int ff_prev = FF_FIRST;
int warmup_stable_retry;

float Temper0;
float TempCoef;

unsigned int freq_stable_cnt = 0;

int Module_Warmup_Time;
char Update_ModuleWarmupTime_flag;
void Update_ModuleWarmupTime(float temper);



void Init_KalmanVariable();

void Init_CONTROL();

void Config_Control();

void InitializeKalmanControl_();
void InitializeKalmanControl();

void Control();
void MainControl();
//void Get_gainA10Mhz();
void InitializePLL();
void WaitToControl();
void LockControl();
void RbSRO_Control();

void CheckControlDAC();

void QuickStartControl(char);
void Wait_QuickStart();
void Init_QuickStart();

void ChangeControlInterval(int interval);

void WaitToControl2();

int FirstDacUpdateCounter;
char FirstDacUpdateFlag;
char SecondDacUpdateFlag;

#define FIRST_DAC_UPDATE_TIME	1800  //At KInterval=50, after 30min.
#define SECOND_DAC_UPDATE_TIME	21600 //At KInterval=50, after 6hr.



int bFirstFreqFilter=1;
int bResetReceiver =0;
double pref_centerdac;

int control_sync_type = 0;

char sync_retry = 0;
char EvenSyncDlyMax = 3;
int iCntEvenSyncDly=0;
int esync_debug=0;
char pll_started = 0;


#define	lock_control			0
#define holdover_control		1
#define recovery_control		2

#define ADCsample_cnt			5
float ADCsample[ADCsample_cnt];
int iSample = 0;

int temp1, temp2;
char turn;
unsigned int next_update;
unsigned int update_cnt = 0;

int prev_ctrl = 0;
int ddac_toggle = 0;

int iControlRetryCnt = 0;
void Init_CONTROL()
{
    // SYSTEM->OPERATION
    // USER

    int i,j;

    /* control */


    CONTROL.CurrentDAC      = 30000;


    /*******************************/
    CONTROL.TimeOffset      = 0;
    CONTROL.PrevTimeOffset  = 0;
    CONTROL.AvgTimeOffset   = 0;
    CONTROL.Slope           = 0;
    CONTROL.PrevSlope       = 0;


    CONTROL.Frequency		= 0;


    CONTROL.IntervalCounter     = 0;
    CONTROL.HoldoverDuration    = 0;

    CONTROL.CV_Control_Counter     = 0;
    /*******************************/

    CONTROL.HigherFraction  = 100;
    CONTROL.LowerFraction   = 100;

    CONTROL.TRACE_SYNC = OFF;
    CONTROL.EVEN_SYNC = OFF;

    CONTROL.PWMhit = 0;


	CONTROL.SURVEY_MODE = EB_3D_FIX;
    CONTROL.WaitToControlCounter = 0;   // CONTROL
	CONTROL.WaitToALMANACCounter = 0;   // STARTUP


    Init_KalmanVariable();


	ControlCounter = 0;


 	Temper0 = 0;
 	TempCoef = 0;

	//#############################################
	// Quick 
	// Control Mode
	// 1. QuickStart -> Normal Control
	//#############################################
	CONTROL.CTRLMODE = CTRL_OCXO_HEATUP_NOT_READY;

    QuickStartCounter = 0;


	FIRST_LOCK = OFF;
	LOCK_REFERENCE = OFF;
	FIRST_STANDBY = ON;


	Init_TRACE_Control();
	Init_HOLDOVER();
	Init_RECOVERY();

	Init_Ctrlmon();


	CONTROL.Run_Mode = FREE_RUN;

	CONTROL.warmup_counter = 0;
	CONTROL.warmup_insertcounter = 0;
	CONTROL.warmup_average = 0;
	
	Module_Warmup_Time = MODULE_WARMUP_TIME_30MIN;	
	
	Update_ModuleWarmupTime_flag = ON;
	
	FirstDacUpdateCounter = 0;
	FirstDacUpdateFlag = ON;
	SecondDacUpdateFlag = ON;
	bDacUpdated = FALSE;
	sync_by_ddac = OFF;
	warmup_stable_retry = 0;
	
	
	CONTROL.warmup_looks_good = 0;

	ChangeControlInterval(Kinterval_STEP_1);	//#120806# sewon
	VME.CV_Data.time				= 0;
	VME.CV_Data.CV_Trans_Period 	= 1;
}


void Config_Control()
{

	int i;

    CONTROL.IntervalCounter     = 0;
    CONTROL.HoldoverDuration    = 0;

    CONTROL.SURVEYSTEP = Survey_UpdateDetailTable;
    //CONTROL.SURVEYSTEP = Survey_Search;


    CONTROL.WarmupStep = 0;


    CONTROL.First = YES;

    CONTROL.LowestDAC       = 0;
    CONTROL.CurrentDAC      = CONFIG.DAC;
    CONTROL.HighestDAC      = 55000;

	CONTROL.RealDAC			= (float)CONFIG.DAC;
	CONTROL.NominalDAC		= (float)CONFIG.DAC;

    CONTROL.CurrentDAC = (int)(CONTROL.RealDAC);

    KGainF=CONFIG.GAIN;

    CONTROL.SYNC = ON;     /* FPGA clear */
	control_sync_type = 9;
    // MATLAB SIMULATION FAST LOCK
    // GAIN SCHEDULING : INIT LOCKING 0 ~ 50 step step!
    // Step_Counter=50 GAIN
    CONTROL.StepCounter = 0;


    Tracking_Interval_Counter = 0;
    DACTableIndex = 0;
    MeanDAC = (double)CONFIG.DAC;
    MeanU = 0;


    for( i = 0; i < MAX_DACTable_SIZE; i++) { DACTable[i] = 0; UTable[i]=0; }
}

void Clear1PPSwithGPS()
{
	//
	char buf[100];
	sprintf(buf,"adjust1pps manual once %d\r\n",(int)(-RAW_OFFSET*100.));
	write(fdEB,buf,strlen(buf));
	printf("<------send cmd:%s\n",buf);
}

void Control()
{
	int t;

    MainControl();
	
    if( CONTROL.SYNC ){ // CONTROL.SYNC ON Clear
    	if(sync_forecast_tod == 0){
    		sync_forecast_tod = 1; 
    		return;
    	}
    	else{
    		sync_forecast_tod++;
    		if(sync_forecast_tod>4){
    			sync_forecast_tod = 0;
    		}
    		else{
    			return;
    		}
    	}

        CONTROL.SYNC = OFF;
        CONTROL.EVEN_SYNC = ON;
        iCntEvenSyncDly = 0;
        sync_retry = 0;


        // Timing Antenna Lock
        if((REFERENCE_MODE == EB_STATUS_RUN) || (REFERENCE_MODE == LMU_STATUS_RUN)
            ||(REFERENCE_MODE == E1_STATUS_RUN) ){
        //if( SYSTEM.EB.STATUS == EB_STATUS_RUN  || lmu.STATUS == LMU_STATUS_RUN){
        	//sewon 12-07-13
        	if(VME.Data.TimeSync_Mode!=VME_ExtClock_MODE)	Clear1PPSwithGPS();
	        else											Clear1PPSwithGPS_Ext1pps();
	        MessageOut("Clear1PPSwithGPS\r\n");
	        CounterMonitorFlag = ON;
			clear_counter_queue();

			if(SYSTEM.E1.STATUS == E1_STATUS_RUN){
				SYSTEM.E1.skip_counter = 10;
			}
			if(SYSTEM.E2.STATUS == E1_STATUS_RUN){
				SYSTEM.E2.skip_counter = 10;
			}
			if(lmu.STATUS == LMU_STATUS_RUN){
				lmu.skip_counter = 10;
			}
			led_sync_retain_cnt=2;
			if(STATUS_Warmup==OFF && 
				(SYSTEM.OPERATION.STATUS == STATUS_ACTIVE || SYSTEM.OPERATION.STATUS == STATUS_STANDBY))
			{
				//LogIt(LOG_CONTROL_SYNC_TYPE, control_sync_type);
				//LogIt(LOG_CONTROL_SYNC,(int)(RAW_OFFSET*100));
				LogIt3(LOG_CATA1_NORM, LOG_CATA2_FAIL, LOG_CONTROL_SYNC, control_sync_type, (int)(RAW_OFFSET), 0);
			}
			warmup_trace_holdup = 0;
	    }
    }
    else if( CONTROL.TRACE_SYNC ) {
        CONTROL.TRACE_SYNC = OFF;
        Clear1PPSWithU1PPS(); //1pps Clear
        //CounterMonitorFlag = ON; -> TRACE EVENT

        //if(STATUS_Warmup==OFF && 
		//		(SYSTEM.OPERATION.STATUS == STATUS_ACTIVE || SYSTEM.OPERATION.STATUS == STATUS_STANDBY))
		//{
			LogIt(LOG_CATA1_NORM, LOG_CATA2_NOTHING, LOG_TRACE_SYNC,(int)(RAW_OFFSET*100));
		//}
		warmup_trace_holdup = 0;
    }
    else if( CONTROL.EVEN_SYNC ) {	// Odd second  clear
        //iCntEvenSyncDly++;
		//if( iCntEvenSyncDly > 15 && (SYSTEM.TIME.CUR.SECOND&0x01) ){
        //	CONTROL.EVEN_SYNC = OFF;
        //	iCntEvenSyncDly = 0;
        //	ClearPP2SWith1PPS();
        //	MessageOut("ClearPP2SWith1PPS\r\n");
        //	
        //	if(STATUS_Warmup==OFF && 
		//		(SYSTEM.OPERATION.STATUS == STATUS_ACTIVE || SYSTEM.OPERATION.STATUS == STATUS_STANDBY))
		//	{
		//		LogIt(LOG_EVEN_SYNC,(int)(RAW_OFFSET*100));
		//	}
        //}
CONTROL.EVEN_SYNC = OFF;
DIAG.ESYNC = OK;
return;
				
        iCntEvenSyncDly++;

        if( iCntEvenSyncDly > EvenSyncDlyMax && (SYSTEM.TIME.CUR.SECOND&0x01) ){
		
			t = ReadSynthCounter(); //kang : Fpga.h--->ReadSynthCounter()--->H/W Dependent
			
			t+=esync_debug;
			
			iCntEvenSyncDly = 0;
			
t = 39321600; //kang
			
			if((t < (39321600-5) || t > (39321600+5)) && pll_started == 1)
			{
				sync_retry++;
				
				CONTROL.EVEN_SYNC = ON;
				
				if(sync_retry == 20)
				{
					//LogIt(LOG_SYNC_ERR,t); 
					CONTROL.EVEN_SYNC = OFF;
					DIAG.ESYNC = FAIL;
				}
				
				if(sync_retry > 20){sync_retry = 100;}
			}
			else
			{
				sync_retry = 0;
				CONTROL.EVEN_SYNC = OFF;
				DIAG.ESYNC = OK;
			}
			
			ClearPP2SWith1PPS();
			//MessageOut("ClearPP2SWith1PPS\r\n");
				
			if(STATUS_Warmup==OFF && 
				(SYSTEM.OPERATION.STATUS == STATUS_ACTIVE || SYSTEM.OPERATION.STATUS == STATUS_STANDBY))
			{
				//LogIt(LOG_EVEN_SYNC,(int)t);
			}

			//EvenSyncDlyMax = 3;
		}
		
    }


}



void Init_KalmanVariable()
{


//    KGainF=GAIN_OF_THE_OCXO;


    // MATLAB SIMULATION FAST LOCK
    KInterval = Kinterval_STEP_3;
	KNextInterval = Kinterval_STEP_3;


	// 98-08-17 8:33
	//Kp = 0.05;
	//Ki = 0.05;
	//T = 0.1;

    // MATLAB SIMULATION FAST LOCK 98-11-22
    // GAIN SCHEDULING : INIT. LOCKING 0 ~ 50 step step!
	// [2000-01-20 14:53]
//    Kp = 0.3;
    Kp = 1.0;
	Ki = 0.05;
	T = 0.1;


    //Kp = 0.1;
	//Ki = 0.025;
 	//T = 0.1;



	TempCoef = -1.5;


	init_matrics(&Pm0,3,3,0);
	init_matrics(&P0,3,3,0);
	init_matrics(&PhiK,3,3,0);
	init_matrics(&QK,3,3,0);
	init_matrics(&GainK,3,1,0);
	init_matrics(&HatX,3,1,0);
	init_matrics(&HatXm0,3,1,0);
	init_matrics(&HK,1,3,0);
	init_matrics(&IK,3,3,0);
    init_matrics(&U,3,1,0);


    /*******************************/
    // 98-08-17 8:35
    // Init_KalmanVariable()
	//USER.R	 = 10;
	//USER.Q1   = 800;
	//USER.Q2   = 10;
	//USER.Q3   = 0.05;

    // MATLAB SIMULATION FAST LOCK 98-11-22
    // KInterval = Kinterval_STEP_3 INIT LOCKING 0 ~ 50 step step!
    USER.R	 = 10;
	USER.Q1   = 2000;
	USER.Q2   = 10;
	USER.Q3   = 0.02;

    USER.R	 = 10;
	USER.Q1   = 800;
	USER.Q2   = 10;
	USER.Q3   = 0.1;

	RK = USER.R;

	Pm0.mat[0][0] = 0;	Pm0.mat[0][1] = 0;	Pm0.mat[0][2] = 0;
	Pm0.mat[1][0] = 0;	Pm0.mat[1][1] = 0;	Pm0.mat[1][2] = 0;
	Pm0.mat[2][0] = 0;	Pm0.mat[2][1] = 0;	Pm0.mat[2][2] = 0;

    //PhiK.mat[0][0] = 0.36787944;	PhiK.mat[0][1] = 0;	PhiK.mat[0][2] = 0;
    // KInterval = Kinterval_STEP_4
    // MATLAB SIMULATION FAST LOCK 98-11-22
    // KInterval = Kinterval_STEP_3
    PhiK.mat[0][0] = 0.01831;	    PhiK.mat[0][1] = 0;	PhiK.mat[0][2] = 0;
    PhiK.mat[1][0] = 0;		        PhiK.mat[1][1] = 1;	PhiK.mat[1][2] = 1;
    PhiK.mat[2][0] = 0;		        PhiK.mat[2][1] = 0;	PhiK.mat[2][2] = 1;

	QK.mat[0][0] = USER.Q1;	QK.mat[0][1] = 0;	    QK.mat[0][2] = 0;
	QK.mat[1][0] = 0;	    QK.mat[1][1] = USER.Q2;	QK.mat[1][2] = 0;
	QK.mat[2][0] = 0;	    QK.mat[2][1] = 0;	    QK.mat[2][2] = USER.Q3;

	IK.mat[0][0] = 1;	IK.mat[0][1] = 0;	    IK.mat[0][2] = 0;
	IK.mat[1][0] = 0;	IK.mat[1][1] = 1;       IK.mat[1][2] = 0;
	IK.mat[2][0] = 0;	IK.mat[2][1] = 0;       IK.mat[2][2] = 1;

	GainK.mat[0][0] = 0;
	GainK.mat[1][0] = 0;
	GainK.mat[2][0] = 0;

	HatX.mat[0][0] = 0;
	HatX.mat[1][0] = 0;
	HatX.mat[2][0] = 0;

	HatXm0.mat[0][0] = 0;
	HatXm0.mat[1][0] = 0;
	HatXm0.mat[2][0] = 0;

	HK.mat[0][0] = 1;	HK.mat[0][1] = 1;	HK.mat[0][2] = 0;
}


void Manual_Control()
{

    double RealDAC_;

	RealDAC_ = CONTROL.ControlDAC;//+OCXO_ID.TDAC;
	//RealDAC_ = 30000;
	//MakePWMInterval(RealDAC_);

   	//CONTROL.CurrentDAC = (int)(RealDAC_);

	if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
       	sprintf((char*)MESG,"%2d:%2d:%2d Manual> R:%7.2f RO:%5.2f RD:%5.2f HEI:%6ld OT:%5.2f c:%5.2f %d\r\n",
           	SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
           	(float)CONTROL.RealDAC, RAW_OFFSET,RAW_DIFFERENCE,SYSTEM.POSITION.CURRENT.HEI,
           	TEMPERATURE.CURRENT,TEMPERATURE.current,GC.ACCsecond);
       	TransmitMessage((BYTE*)MESG,DEBUG_PORT);
    }
}

void MainControl()
{
    double RealDAC_;
    unsigned short uDAC;
	
	//kang
	int deltaDAC;

    char c1,c2;
    int t1;
    int t2;
    int ff_result;
	
	int i;
	float ADCsum = 0;
	float ADCavg = 0;
	float ADCvar = 0;
	float ADCdev = 0;
	char *s1,*s2,*s3;

	if(USER.MANUAL_CONTROL == ON){
	    Manual_Control();
	    return;
	}

//kang----------------------------------------------------------------------------------------
	else if(OCXOID.TraceControl == ON){
		TraceControl_ExtRef();
		
		//Writing OCXO-ID Log -------------------------------------------------------------
		OCXOID.cnt++;
		if( OCXOID.cnt >= 1 ){
			OCXOID.cnt = 0;
			printf(	"%4d/%2d/%2d %2d:%2d:%2d; %7.2f; %4.2f; %4.2f; "
					"%5.2f, %5d, "
					"%08d, %d, %d, %d, %d, %d, "
					"%.2f, %.2f, %.2f, %.1f, %.2f\r\n"
					,SYSTEM.TIME.CUR.YEAR, SYSTEM.TIME.CUR.MONTH, SYSTEM.TIME.CUR.DAY
					,SYSTEM.TIME.CUR.HOUR, SYSTEM.TIME.CUR.MINUTE, SYSTEM.TIME.CUR.SECOND
					,(float)CONTROL.RealDAC, (float)ts.ocxo_temp, (float)ts.board_temp
					,(float)OCXO_ID.TDAC, (int)CONTROL.CurrentDAC
					,GetCounterTraceAll(), OCXOID.detailSense, OCXOID.detail, OCXOID.DetailOffset, TRACE.TimeOffset, OCXOID.trace_interval
					,TRACE_KP, TRACE_KI, TRACE_T, OCXOID.ro_sum, OCXOID.aveoffset
			);
			
		}
		return;
	}
//kang-----------------------------------------------------------------------------------------


	if( GC.RunningTime < 3*3600 ){
		switch(	GC.RunningTime ){
			case 600:   // 10 min.
			case 1800:  // 30 min.
			case 3600:  // 1 hr.
			case 7200:  // 2 hr.
			case 10800: // 3 hr.

                LogEbStatus();
				break;
			default:
				break;
		}
	}
	else if( GC.RunningTime <= 24*3600 ){
		if(	!( GC.RunningTime % (3*3600) ) ){
			LogEbStatus();
			next_update = (24+25)*3600;
		}
	}
	else{
		//if( !( GC.RunningTime % (23*10*3600) ) ){
		if(GC.RunningTime == next_update){
			update_cnt++;
			
			if(update_cnt>10){update_cnt = 100;	next_update += 241*3600;}
			else next_update += 25*3600;
				
			LogEbStatus();
		}
	}
	
	monitor_traim();
	
    switch(SYSTEM.OPERATION.OPMODE){

        case OPMODE_WARMUP:
				
				//if(SYSTEM.EB.STATUS == EB_STATUS_RUN){
				//	//if( SYSTEM.EB.LOCKMODE == EB_UNLOCKED || CHECKLOCK.HoldoverDelayCounter > 0){
				//	if( SYSTEM.EB.LOCKMODE == EB_UNLOCKED ){
           		//		WaitToControl();
           		//		return;
           		//	}
           		//}


				if(DIAG.OSC == FAIL){
       				WaitToControl2();
       				return;
				}

				Warmup_Trace_Control();

				dbgCV(123) printf("\n>>>>>>%d, %d"
						", %d, %d\n",
						CONTROL.SURVEYSTEP,SYSTEM.EB.STATUS ,
						SYSTEM.EB.LOCKMODE, SYSTEM.TIME.Valid);
               switch(CONTROL.SURVEYSTEP){
                //WARM-UP Detail Counter Update
                //Survey_UpdateDetailTable; control.h
				
                	case Survey_UpdateDetailTable:
						
							SYSTEM.OPERATION.DETAIL_MODE = 0;
							CONTROL.SURVEYSTEP = Survey_Search;
							break;

                    case Survey_Search:
                            Wait_QuickStart();
                            break;
                    case Survey_InitializePLL:
                    		if( StartUpSet() != OK) {
           						break;
           					}

                    		if( ((VME.Data.TimeSync_Mode == VME_ExtClock_MODE)||(VME.Data.TimeSync_Mode == VME_PTP_MODE)) ){
                    			ForceGPSTracked();
//                    			SetEbDummy();
                    						}
                    		//if(SYSTEM.EB.STATUS == EB_STATUS_RUN){
								//if( SYSTEM.EB.LOCKMODE == EB_UNLOCKED || CHECKLOCK.HoldoverDelayCounter > 0){
								//if( SYSTEM.EB.LOCKMODE == EB_UNLOCKED ){
									if( (SYSTEM.EB.STATUS != EB_STATUS_RUN) || (SYSTEM.EB.LOCKMODE == EB_UNLOCKED) ){
           							WaitToControl();
           							break;
           						}
           					//}
           					
                            InitializePLL();
                            break;
                }

                CONTROL.WaitToControlCounter = 0;
                break;

        case OPMODE_OPERATION:
                if((REFERENCE_MODE == E1_STATUS_RUN) && (gCurrent>1000 || gCurrent<-1000) ){
                     //CONTROL.SYNC = ON;
                }
				//##################################################################
				// Trace
				//##################################################################
                if(SYSTEM.OPERATION.CTRLMODE ==	CTRL_LOCKEDtoMASTER){
					SetDetailOutputTraceCounter();

					TraceControl();

					if( (SYSTEM.OPERATION.STATUS == STATUS_STANDBY) && (OSC.kind == OCXO_OSC) && (CounterModule.osc == Rubidium_OSC) && !(TRACE.ControlCounter % 3600 ) && (DIAG.SYSTEM == OK) ) {
						uDAC = (unsigned short)TRACE.MeanDAC;
						//CONFIG.DAC = (unsigned short)TRACE.MeanDAC;
				    	if( (uDAC) < EFC_UPPER_LIMIT && (uDAC) > EFC_LOWER_LIMIT ){
							if(uDAC != CONFIG.DAC){
								CONFIG.DAC = uDAC;
//								LogIt(LOG_UPDATE_DAC,(int)CONFIG.DAC);
								CONFIG.WriteEvent = ON;
							}
						}
					}
   				}
   				else{
   					SetDetailOutputMainCounter();
					funcReadTraceCounter();
   				}

                if(SYSTEM.OPERATION.LOCKMODE == LOCKED){
    				if( RcvAlarm.ALARM_CONTROL == ON ){
     					turn = holdover_control;
     					FirstDacUpdateCounter = 0;
						control_mode = MODE_HLD1;
     				}
     				else if((err_10m_cnt>=2) || (waiting_change_eb) || (skipping_change_eb)){
 						turn = holdover_control;
 						control_mode = MODE_HLD2;
 					}
     				else if ( RECOVERY.flag == ON ) {
     					turn = recovery_control;
     					FirstDacUpdateCounter = 0;
     					control_mode = MODE_RCV1;
     				}
     				else if(SYSTEM.EB.LOCKMODE == EB_UNLOCKED) {
 						turn = holdover_control;
 						control_mode = MODE_HLD3;
 					}
 					else if(fp_err_flag){
 						fp_err_flag = OFF;
 							
						RECOVERY.flag = ON;
						RECOVERY.IntervalCounter = 0;
						RECOVERY.wait_time = 10;
						RECOVERY.average_time = 20;
							
 						InitializeKalmanControl();
 						CONTROL.StepCounter = 0;
						ChangeControlInterval(Kinterval_STEP_1);
							
						CONTROL.ControlDAC = CONTROL.RealDAC = CONTROL.PrevDAC = (double)CONFIG.DAC;
						MeanDAC = CONTROL.NominalDAC = CONTROL.RealDAC;
							
 						turn = recovery_control;
 						FirstDacUpdateCounter = 0;
 						control_mode = MODE_RCV2;
 					}
     				else {
     					turn = lock_control;
						if(KInterval>=Kinterval_STEP_3){FirstDacUpdateCounter++;}
     					//if(FirstDacUpdateCounter>=FIRST_DAC_UPDATE_TIME){
     					if(FirstDacUpdateCounter>=SECOND_DAC_UPDATE_TIME){
     						FirstDacUpdateCounter = SECOND_DAC_UPDATE_TIME;
     					}
	 					control_mode = MODE_LOCK;
     				}
                }
                else {
                	turn = holdover_control;
                	FirstDacUpdateCounter = 0;
	            	control_mode = MODE_HLD4;
                }


				if(SYSTEM.OPERATION.CTRLMODE ==	CTRL_LOCKEDtoGPS)
				{
					ff_result = ffPush(
						((!control_mode) && (SYSTEM.EB.LOCKMODE!=EB_UNLOCKED) && (KInterval==Kinterval_STEP_3)),
		            	RAW_OFFSET,
		            	CONTROL.RealDAC,
		            	TEMPERATURE.CURRENT);
	
		            if(ff_prev != ff_result){
		            	if(ff_prev == FF_STABLE && ff_result == FF_UNSTABLE && CONFIG.RcvType==1){
		            		LogIt3(LOG_CATA1_NORM, LOG_CATA2_WARN, LOG_SYNC15_FREQ, 
		            			((FF_UNSTABLE<<16)&0xFFFF0000) | (((int)CONTROL.RealDAC)&0xFFFF),
		            			TEMP_RAW_DIGITS, 
		            			(int)(ffD.Freq*1E11));
	
		            		LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, 
		            			LOG_SYNC15_RESET, RESET_MODEL_BOTH, RESET_TYPE_INITSTEP, 0);
		            			
		            		skipping_change_eb = 1;
	
		            		Init_EB(INIT_EB_SILENT, INIT_EB_SECOND, YES);
							EB_Task_start();
								
							ff_prev = ff_result;
							
		            	}
		            	else if(ff_result == FF_STABLE){
		            		LogIt3(LOG_CATA1_NORM, LOG_CATA2_OK, LOG_SYNC15_FREQ, 
		            			((FF_STABLE<<16)&0xFFFF0000) | (((int)CONTROL.RealDAC)&0xFFFF),
		            			TEMP_RAW_DIGITS, 
		            			(int)(ffD.Freq*1E11));
	
		            		ff_prev = ff_result;
		            	}
		            }

				}
            	switch(turn) {

            		case lock_control:
            							if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
											sprintf(MESG,"~L%d(%d)~",KInterval,ff_result);
											TransmitMessage((BYTE*)MESG,PORT_CONTROL);
        	                            }


                                    	LockControl();
	
                                    	//#############################################################
										//On Lock DAC
										//#############################################################

										if( (FirstDacUpdateCounter >= FIRST_DAC_UPDATE_TIME) && (FirstDacUpdateFlag == ON) && (DIAG.SYSTEM == OK)){
											
											FirstDacUpdateFlag = OFF;
										//if( (FirstDacUpdateCounter >= 30) && (FirstDacUpdateFlag == ON) && (DIAG.SYSTEM == OK)){
										//	FirstDacUpdateCounter = 0;
										//	FirstDacUpdateFlag = OFF;
											//LogIt3(LOG_UPDATE_DAC,(int)CONFIG.DAC, (int)CONTROL.RealDAC, GC.RunningTime);
											LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, 
												LOG_UPDATE_DAC,(int)CONFIG.DAC, (int)CONTROL.RealDAC, 1);
											CONFIG.DAC = (int)CONTROL.RealDAC;
											CONFIG.WriteEvent = ON;
											bDacUpdated = TRUE;
										}

										if( (FirstDacUpdateCounter >= SECOND_DAC_UPDATE_TIME) && (SecondDacUpdateFlag == ON) && (DIAG.SYSTEM == OK)){
											SecondDacUpdateFlag = OFF;
											//LogIt3(LOG_UPDATE_DAC,(int)CONFIG.DAC, (int)CONTROL.RealDAC, GC.RunningTime);
											LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, 
												LOG_UPDATE_DAC,(int)CONFIG.DAC, (int)CONTROL.RealDAC, 2);
											CONFIG.DAC = (int)CONTROL.RealDAC;
											CONFIG.WriteEvent = ON;
											bDacUpdated = TRUE;
										}

										//if(KInterval>=Kinterval_STEP_3 && ControlCounter<262144){

										if(KInterval>=Kinterval_STEP_3 && ControlCounter<262800){
											ControlCounter++;
										}
										//#############################################################
										// DAC
										//#############################################################
										
										//kang
										deltaDAC = abs( (int)CONFIG.DAC - (int)CONTROL.RealDAC );
										
										//kang if( (ControlCounter >= 262800 ) && (DIAG.SYSTEM == OK) ){
										if( SYSTEM.OPERATION.CTRLMODE !=	CTRL_LOCKEDtoMASTER &&
										( (ControlCounter >= 262800) || (deltaDAC>100 && ff_result == FF_STABLE) ) && (DIAG.SYSTEM == OK) ){
											ControlCounter = 0;
											//LogIt3(LOG_UPDATE_DAC,(int)CONFIG.DAC, (int)CONTROL.RealDAC, GC.RunningTime);
											LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, 
												LOG_UPDATE_DAC,(int)CONFIG.DAC, (int)CONTROL.RealDAC, 3 );
											CONFIG.DAC = (int)CONTROL.RealDAC;
											CONFIG.WriteEvent = ON;
											bDacUpdated = TRUE;
										}


										if(STATUS_Warmup == OFF){
											LOCK_REFERENCE = ON;
										}

										break;

					case holdover_control:
										if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
        	                            	sprintf(MESG,"~H%d(%d)~",control_mode,ff_result);
        	                            	TransmitMessage((BYTE*)MESG,PORT_CONTROL);
        	                            }
        	                            
	                                    HoldoverControl();
										// Locked Count
        	                            ControlCounter = 0;

        	                            

        	                            RcvAlarm.offset_index = 0;
        	                            break;

					case recovery_control:
										if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
											sprintf(MESG,"~R%d(%d)~",control_mode,ff_result);
        	                            	TransmitMessage((BYTE*)MESG,PORT_CONTROL);
        	                            }

										Recovery();
										
										RcvAlarm.offset_index = 0;
										break;

            	}

				// First_Lock Control
				if( !FIRST_LOCK ){
					QuickStartControl(turn);
				}

				if( CounterMonitorFlag == ON ) {
					MointorCounterClear();
				}


				RealDAC_ = CONTROL.ControlDAC+OCXO_ID.TDAC;

    			MakePWMInterval(RealDAC_);
    			CONTROL.CurrentDAC = (int)(RealDAC_);
                break;

    }

	//switch(CounterModule.STATUS){
	//	case NOT_EXIST:				s1 = "NONE"; 		break;
	//	case STATUS_ACTIVE:			s1 = "ACT"; 		break;
	//	case STATUS_BLOCK:			s1 = "BLK"; 		break;
	//	case STATUS_STANDBY:		s1 = "STB"; 		break;
	//	case STATUS_PRE_STANDBY:	s1 = "PRE"; 		break;
	//	default:					s1 = "NULL"; 		break;
	//}
	//		
	//switch(CounterModule.LockMode){
	//	case LOCKED:				s2 = "LOCK"; 		break;
	//	case RECOVERING:			s2 = "RECOVERY"; 	break;
	//	case COASTING:				s2 = "COAST"; 		break;
	//	case TIME_LOCK:				s2 = "TIME LOCK"; 	break;
	//	default:					s2 = "NULL";		break;
	//}
	//
	//
	//
	//if (dual_print_9927){
	//	sprintf(
	//		(char*)MESG,
	//		"     dual>> Exist:%d S:%s L:%s P:%d\n",
	//		CounterModule.EXIST,
	//		s1,
	//		s2,
	//		CounterModule.priority
	//		
	//	);
	//									
    //    TransmitMessage(MESG,DEBUG_PORT);
    //}



	if(SYSTEM.OPERATION.OPMODE == OPMODE_OPERATION){

		if(turn == holdover_control){
			c1 = 'H';
			t1 = CONTROL.HoldoverDuration;
			t2 = 3;
		}
		else if(turn == recovery_control){
			c1 = 'R';
			t1 = CONTROL.IntervalCounter;
			t2 = 2;
		}
		else if(turn == lock_control){
			c1 = 'L';
			t1 = CONTROL.IntervalCounter;
			t2 = 1;
		}
	}
	else {
			c1 = 'W';
			t1 = CONTROL.IntervalCounter;
			t2 = 0;
	}


    if (monitor.holdover){
       	sprintf((char*)MESG,"%2d:%2d:%2d [%c]; %3d; %7.2f; %7.2f; %7.2f; "
       											"%5.2f; %d; %5.2f; %5.2f; %5.2f; %5.2f; "
       											"%4d; %.2f; %.4f; %.4f; %d; %d; %d; %d; %d\r\n",
                		SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
                		c1,
                		t1,
                		(float)CONTROL.RealDAC,(float)MeanDAC,OCXO_ID.TDAC,
                		(float)RAW_OFFSET, uiTemp,
                		(float)TEMPERATURE.CURRENT,(float)TEMPERATURE.AVERAGE,
                		(float)TEMPERATURE.current,(float)TEMPERATURE.current_average,
                		HoldoverCONTROL.AgingTableSize,
                		(float)Aging_sol[0],(float)Aging_sol[1],(float)Aging_sol[2],
                		GC.ACCsecond,SYSTEM.EB.TRAIM.ACCURACY,RcvAlarm.alarm_flag,
                		t2, SYSTEM.TRACK.TRACKED);
   		TransmitMessage((BYTE*)MESG,DEBUG_PORT);
	}
}

void CheckControlDAC()
{

	char	dDAC;
	double	comp1,comp2;
	
	if( CONTROL.RealDAC < 5 ) {CONTROL.RealDAC = 5;}
	if( CONTROL.RealDAC > 65530) {CONTROL.RealDAC = 65530;}

	//DeltaDAC = CONTROL.PrevDAC - CONTROL.RealDAC;
	DeltaDAC = CONTROL.RealDAC - CONTROL.PrevDAC;
	
	comp1 = 100.0*((double)KGainF*(double)KInterval);
	comp2 = -comp1;
	
	if(SYSTEM.OPERATION.OPMODE == OPMODE_OPERATION && STATUS_Warmup==OFF && 
		(SYSTEM.OPERATION.STATUS == STATUS_ACTIVE || SYSTEM.OPERATION.STATUS == STATUS_STANDBY)){
		if(KInterval==Kinterval_STEP_1 && DeltaDAC>100.0) {
			if(Modified_RAW_OFFSET>10 && average_rd_last>0.0){
				InitializeKalmanControl();
			}
			else{
				if(average_rd_last<= 0.2){GPSCurrentU = GPSPrevU+comp1;}
				else{GPSCurrentU = GPSPrevU;}
			}
				
			CONTROL.RealDAC = GPSCurrentU/((double)KGainF*(double)KInterval) + CONTROL.NominalDAC;
		}
		else if(KInterval==Kinterval_STEP_1 && DeltaDAC<-100.0) {
			if(Modified_RAW_OFFSET<-10 && average_rd_last<0.0){
				InitializeKalmanControl();
			}
			else{
				if(average_rd_last>= -0.2){GPSCurrentU = GPSPrevU+comp2;}
				else{GPSCurrentU = GPSPrevU;}
			}
			
			CONTROL.RealDAC = GPSCurrentU/((double)KGainF*(double)KInterval) + CONTROL.NominalDAC;
		}
		else if(KInterval==Kinterval_STEP_2 && (DeltaDAC>500.0 || DeltaDAC<-500.0)){
			InitializeKalmanControl();
		}
		else if(KInterval==Kinterval_STEP_3 && (DeltaDAC>1000.0 || DeltaDAC<-1000.0)){
			InitializeKalmanControl();
		}
	}

	DeltaDAC = CONTROL.RealDAC - CONTROL.PrevDAC;

	//if( CONTROL.CTRLMODE == CTRL_NORMAL ) {
	//	if(DeltaDAC > 4000 || DeltaDAC < -4000 ) {
	//		dDAC = FAIL;
	//	}
	//	else {
	//		dDAC = OK;
	//	}
	//}
	//else {
		//if ( DeltaDAC > 5000 || DeltaDAC < -5000 ) {
		if ( DeltaDAC > 10000 || DeltaDAC < -10000 ) {
			dDAC = FAIL;
		}
		else {
			dDAC = OK;
		}
	//}

	if(dDAC == FAIL){

		//TLogIt(TLOG_dDAC_Fail,(int)DeltaDAC);

		switch(SYSTEM.OPERATION.CTRLMODE){

			case	CTRL_LOCKEDtoMASTER:
						CONTROL.TRACE_SYNC = ON;
						break;

			case	CTRL_LOCKEDtoGPS:
						//CONTROL.SYNC = ON;
						//control_sync_type = 8;
						break;
		}

        if((REFERENCE_MODE == E1_STATUS_RUN) || (REFERENCE_MODE == E2_STATUS_RUN)){
			RECOVERY.flag = ON;
			RECOVERY.IntervalCounter = 0;

			RECOVERY.wait_time = 3;
			RECOVERY.average_time = 5;
			Recovery_Offset = 0;
			CONTROL.SYNC = ON;
			control_sync_type = 7;
		}
		

		DIAG.dDAC = FAIL;
       	if (PrevDIAG.dDAC == OK ) {
			log_ddac_fail();
			
			if( CONFIG.RcvType && SYSTEM.OPERATION.OPMODE == OPMODE_OPERATION &&
				STATUS_Warmup==OFF && 
				(SYSTEM.OPERATION.STATUS == STATUS_ACTIVE || SYSTEM.OPERATION.STATUS == STATUS_STANDBY))
			{
				LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, LOG_SYNC15_RESET, RESET_MODEL_BOTH, RESET_TYPE_INITSTEP, 1);
      			Init_EB(INIT_EB_SILENT, INIT_EB_SECOND, YES);
				EB_Task_start();
			}
			else{
				if(KInterval==Kinterval_STEP_1 && ddac_toggle==0){CONTROL.SYNC = ON; ddac_toggle=1;}
				else{
					ddac_toggle=0;
					if(CONFIG.RcvType==1 || 1){
						LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, LOG_SYNC15_RESET, RESET_MODEL_BOTH, RESET_TYPE_INITSTEP, 1);
						Init_EB(INIT_EB_SILENT, INIT_EB_SECOND, YES);
						EB_Task_start();
					}
				}
			}

       		//sync_by_ddac = ON;
       	}

		InitializeKalmanControl();

		//ChangeControlInterval(Kinterval_STEP_1);
		
		if( SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoMASTER ){
			InitializeTraceControl();
		}


		CONTROL.ControlDAC = CONTROL.RealDAC = CONTROL.PrevDAC = (float)CONFIG.DAC;
		MeanDAC = CONTROL.NominalDAC = CONTROL.RealDAC;
		
		
	}
	else  {
		CONTROL.ControlDAC = CONTROL.RealDAC;
		DIAG.dDAC = OK;
		if (PrevDIAG.dDAC == FAIL ) {
       		log_ddac_ok();
       	}
	}

/*

	if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
       	sprintf((char*)MESG," %d) D:%7.2f %7.2f %7.2f %d\r\n",
           	CONTROL.CTRLMODE,DeltaDAC,CONTROL.PrevDAC,CONTROL.RealDAC,dDAC);
       	TransmitMessage((BYTE*)MESG,DEBUG_PORT);
    }
*/
}




//#########################################################################
// 3D-fix
//#########################################################################
void WaitToControl()
{

	CONTROL.WaitToControlCounter++;

	if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
		sprintf((char*)MESG,"Waiting GPS mode...%d,%d) %d %d %d %d\r\n"
			,SYSTEM.EB.STATUS == EB_STATUS_RUN , SYSTEM.EB.LOCKMODE == EB_UNLOCKED
			,SYSTEM.EB.STATUS,SYSTEM.EB.LOCKMODE,CHECKLOCK.HoldoverDelayCounter,CHECKLOCK.LockedCounter);
		TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	}
}



void WaitToControl2()
{

	CONTROL.WaitToControlCounter++;

	if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
		sprintf((char*)MESG,"Waiting OSC good ...\r\n");
		TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	}
}



void InitializePLL()
{

	int i;
    double RealDAC_;

    ++CONTROL.IntervalCounter;

    if(CONTROL.IntervalCounter == 1 ){

		// ReSync  Kalman Variables
		// [2000-01-20 15:48]
		Init_KalmanVariable();

		// Kalman Variables
        //HatXm0.mat[0][0] = HatXm0.mat[1][0] = 0;
		U.mat[0][0] = U.mat[2][0] = 0;

		//RealDAC = CONTROL.RealDAC;

		CONTROL.PrevTimeOffset = 0;
		CONTROL.PrevDAC = CONTROL.NominalDAC;

		GPSCurrentU = GPSPrevU = 0.0;

	    TRACE.CurrentU = TRACE.PrevU = 0.0;
	    TRACE.PrevAvgTimeOffset = 0.0;
	    TRACE.AvgTimeOffset = 0.0;
        TRACE_OFFSET = TRACE_DIFFERENCE = 0;
	    TRACE.ControlCounter = 0;


	    Temper0 = TEMPERATURE.CURRENT;

		OCXO_ID.TDAC = 0;

		//===================================================================
		// Temperature Moving Average [99-11-03 9:22��
		//===================================================================

		// [2000-01-18 23:29]
		if( FIRST_LOCK == OFF ){
			OCXO_ID.TDAC = 0;
		}

		CONTROL.ControlDAC = MeanDAC = TRACE.MeanDAC = CONTROL.RealDAC;
		TRACE.NominalDAC = CONTROL.NominalDAC = CONTROL.RealDAC;

		RealDAC_ = CONTROL.ControlDAC+OCXO_ID.TDAC;
		MakePWMInterval(RealDAC_);
		CONTROL.CurrentDAC = (int)(RealDAC_);

		CONTROL.SYNC = ON;
		control_sync_type = 6;
		pll_started = 1;
		MessageOut("1PPS Sync.!!\r\n");

	}

	else if ( CONTROL.IntervalCounter == 2 ) {
		//===============================================
		// 1PPS OCXO!!!!!!!!!!
		//Select1PPS_OCXO();
		MessageOut("Select OCXO!!\r\n");
		
		//Select500nsForDetail();	//Center
		//kang SelectCenterForDetail_50ns();
		
		SYSTEM.OPERATION.DETAIL_MODE = 1;
	}

	else if ( CONTROL.IntervalCounter == 4 ) {
		//kang usCenterADC = usADCDetail;
		
		//Select1usForDetail();	// end
		//kang SelectEndForDetail_50ns();
		
		//kang if( usCenterADC < 50 || usCenterADC >200) CONTROL.IntervalCounter = 1;
		
	}

	else if ( CONTROL.IntervalCounter == 6 ) {
		//kang usMaxADC = usADCDetail;
		
		
		//kang MakeDetailTable(ssDetailTable,usCenterADC,usMaxADC,DETAIL_50);	// Center, end 8
		
		if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
			sprintf((char*)MESG,"Control.IntervalCounter Center: %d Max:%d==>Min:%d \r\n"
							,usCenterADC,usMaxADC,usMaxADC-usCenterADC);
			TransmitMessage((BYTE*)MESG,DEBUG_PORT);
		}


		//Select1PPSEBForDetail();
		
		//kang SelectU1PPSForDetail_50ns();
		MessageOut("Make Detail Counter!!\r\n");
		
		//kang if( usMaxADC < 200 || usMaxADC >300) CONTROL.IntervalCounter = 1;
	}

	else if ( CONTROL.IntervalCounter == 7 ) {

		SYSTEM.OPERATION.DETAIL_MODE = 0;
		EVENT.PLL = ON;
		MessageOut("PLL Start!\r\n");
	}

	else if ( CONTROL.IntervalCounter >= 12 ) {

       	SYSTEM.OPERATION.OPMODE = OPMODE_OPERATION;
		LogItString(LOG_CATA1_NORM, LOG_CATA2_NOTHING, "Operation started");
    	//SYSTEM.OPERATION.LOCKMODE = LOCKED;

       	CONTROL.IntervalCounter = 0;
       	CONTROL.TimeOffset = 0;

	    KInterval = Kinterval_STEP_1;
		KNextInterval = Kinterval_STEP_1;        //

       	// Reset General Counter to vary Operation Interval ....
       	GC.Hour = 0;
       	GC.Second = 0;
        //ChangeControlInterval(Kinterval_STEP_2);

    }
    else{
    }
}








/* ---------------------------------------------------------------------
Last Updated  : 98-02-27 11:03�?
                [2000-01-18 22:45]

Function Name : Operation

Description   : GPS Locked
                Master Locked f�? GPS Locked controller�?GPS 1PPS
                Master tracking controller
                Master tracking LED ON

Parameters    :
Return Values :
--------------------------------------------------------------------- */
void LockControl()
{

	int i,count,index;
	int DACNo;
	double DACsum, Usum;
	char	message[30];
	int detail_time;



    CONTROL.HoldoverDuration = 0;   //HOLDOVER
    CONTROL.IntervalCounter++;

    recovery_duration = 0;

	if( SKIP_CONTROL == ON){
		RcvAlarm.offset_index = 0;
	}
	else {
		RcvAlarm.alarm_start_time = GC.RunningTime;
	}


    // GPS Locked control ime Interval
    // Height
    // Modified_RAW_OFFSET
//    Modified_RAW_OFFSET = RAW_OFFSET - (SYSTEM.POSITION.CURRENT.HEI-SYSTEM.POSITION.AVERAGE.HEI)/4243.0;
    Modified_RAW_OFFSET = RAW_OFFSET;

	if(KInterval<Kinterval_STEP_3){
		freq_stable_cnt=0;
		freq_stable_enough = NO;
		RcvAlarm.offset_index = 0;
	}

	if(CHECKLOCK.UnlockedCounter == 0 && KInterval >= Kinterval_STEP_3 && freq_stable_enough){
		CheckReceiverAlarm((int)(Modified_RAW_OFFSET*100.0));
	}


    if ((SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS) && (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG)){

        //sprintf((char*)MESG,"%3d:%2d:%2d:%2d %3u> R:%7.2f M:%7.2f RO:%5.2f RD:%5.2f H:%6ld T:%5.2f TD:%5.2f %d %s\r\n",
        //        	CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
        //          	CONTROL.IntervalCounter,CONTROL.RealDAC,MeanDAC,RAW_OFFSET,RAW_DIFFERENCE,SYSTEM.POSITION.CURRENT.HEI,
        //          	TEMPERATURE.CURRENT,OCXO_ID.TDAC,GC.ACCsecond,OCXO_ID.id_str);
        
        sprintf((char*)MESG,"%3d:%2d:%2d:%2d %3u>"
        						" R:%7.2f M:%7.2f RO:%5.2f,%5.2f, RD:%5.2f H:%6ld T:%5.2f TD:%5.2f %d [%d]\r\n",
                	CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,CONTROL.IntervalCounter
                  	,CONTROL.RealDAC,MeanDAC,RAW_OFFSET,g_detail,RAW_DIFFERENCE,SYSTEM.POSITION.CURRENT.HEI,
                  	(float)TEMPERATURE.CURRENT,(float)OCXO_ID.TDAC,GC.ACCsecond,IsSigOn());

        TransmitMessage((BYTE*)MESG,DEBUG_PORT);
    }


    if (/*(SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS) &&*/ (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG_DETAIL2)){

       	sprintf((char*)MESG,"%3d:%2d:%2d:%2d %3u> R:%7.2f M:%7.2f RO:%5.2f RD:%5.2f H:%6ld T:%5.2f C:%5.2f TD:%5.2f %d %s,m,%.9le,r_o,%.9le,gCA,%.9le\r\n",
                	CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
                  	CONTROL.IntervalCounter,CONTROL.RealDAC,MeanDAC,RAW_OFFSET,RAW_DIFFERENCE,SYSTEM.POSITION.CURRENT.HEI,
                  	(float)TEMPERATURE.CURRENT,(float)TEMPERATURE.current,(float)OCXO_ID.TDAC,
                  	GC.ACCsecond,OCXO_ID.id_str,ModiValue,Recovery_Offset,gCurrentAll);

        TransmitMessage((BYTE*)MESG,DEBUG_PORT);
    }


	//##################################################
	// Detail Counter
	//##################################################

	//2004-03-23 4:25
	//kang detail_time = GC.RunningTime%86400;
	detail_time = 0;

	//2004-03-09 2:14
//	if(SYSTEM.OPERATION.CTRLMODE != CTRL_LOCKEDtoMASTER){
//		switch(detail_time){
//			case 1:
//				SYSTEM.OPERATION.DETAIL_MODE = 1;
//				//kang SelectCenterForDetail_50ns();
//				return;
//			case 3:
//				//kang usCenterADC = usADCDetail;
//				//kang SelectEndForDetail_50ns();
//				return;
//			case 5:
//				//kang usMaxADC = usADCDetail;
//
//				//kang MakeDetailTable(ssDetailTable,usCenterADC,usMaxADC,DETAIL_50);
//
//
//				//Select1PPSEBForDetail();
//				//kang SelectU1PPSForDetail_50ns();
//
//			    if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
//	       			sprintf((char*)MESG,"Counter:Center: %d Max:%d\r\n",(int)usCenterADC, (int)usMaxADC);
//			        TransmitMessage((BYTE*)MESG,DEBUG_PORT);
//	    		}
//				return;
//			case 6:
//				SYSTEM.OPERATION.DETAIL_MODE = 0;
//				return;
//		}
//	}
	/////////////////////////////////////////////////////////////////////////

	if( KInterval == Kinterval_STEP_3 ){

		/*if( CONTROL.IntervalCounter == 1) {
			//YUN
			//Select500nsForDetail();	return;
			SYSTEM.OPERATION.DETAIL_MODE = 1;
			SelectCenterForDetail_50ns(); return;

		}
		else if( CONTROL.IntervalCounter == 2) {
			usCenterADC = usADCDetail;
			//YUN
			//Select1usForDetail();	return;
			SelectEndForDetail_50ns(); return;
		}
		else if( CONTROL.IntervalCounter == 4) {
			usMaxADC = usADCDetail;
			//YUN
			MakeDetailTable(ssDetailTable,usCenterADC,usMaxADC,DETAIL_50);

			Select1PPSEBForDetail();

		    if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
       			sprintf((char*)MESG,"Counter:Center: %d Max:%d\r\n",usCenterADC,usMaxADC);
		        TransmitMessage((BYTE*)MESG,DEBUG_PORT);
    		}
			return;
		}*/
	}

/*
	//##################################################
	// First_Lock Control
	//
	if( !FIRST_LOCK ){
		QuickStartControl();
	}
*/
    //RAW_OFFSET -> Modified_RAW_OFFSET
	//if(CHECKLOCK.UnlockedCounter == 0 ){
	    InsertTimeOffset(&TimeOffsetTable,(int)(Modified_RAW_OFFSET*100),(int)(RAW_DIFFERENCE*100));
	    average_rd_now = average_rd_now + RAW_DIFFERENCE;
	    if(RAW_DIFFERENCE > 0.1) {average_rd_plus++;}
		if(RAW_DIFFERENCE < -0.1) {average_rd_minus++;}
	//}

	//==============================================
	//KInterval(default Kinterval_STEP_4)
//    if( !(GC.ACCsecond%KInterval) ){
//	if( TimeOffsetTable.InsertCount >= KInterval){
	if( ((IsCV_Slave())&&(CONTROL.CV_Control_Counter++==2)) ||
			((!IsCV_Slave())&&(TimeOffsetTable.InsertCount>=KInterval)) ){	//#120820# sewon


		average_rd_last = average_rd_now / KInterval;
		
		
		//sprintf(MESG,"\r\np:%d, m:%d, Last:%f NOW:%f", 
		//average_rd_plus, average_rd_minus, average_rd_last, average_rd_now);
		//MessageOut(MESG);
		
		average_rd_now = 0.0;
		
   		CONTROL.StepCounter++;

        CONTROL.AvgTimeOffset = (double)((float)(GetTimeOffset(&TimeOffsetTable,GPS_CUTOFF))/100.0) * 0.0000001;

        	CONTROL.IntervalCounter = 0;  // DISPLAY


		RcvAlarm.start_flag = ON;
		//
		if( SKIP_CONTROL ){
			SKIP_CONTROL = OFF;			
			average_rd_plus = average_rd_minus = 0;
			return;
		}
		
		if(SKIP_CALC){
			SKIP_CALC = OFF; 
			average_rd_plus = average_rd_minus = 0;
			return;
		}

		//=========================================================================
		// Aging Table
		//  CONTROL.StepCounter
		//  SkipAgingTableCounter Recovery
		//  Aging
		//  [2000-01-18 23:31]
		//=========================================================================
		if( SkipAgingTableCounter == 0 ) {
			InsertAgingTable(CONTROL.StepCounter,(float)CONTROL.RealDAC,(float)TEMPERATURE.AVERAGE);
			if( KInterval > (Kinterval_STEP_2+Kinterval_STEP_3)/2 ){
				TGainPushEach((int)GC.RunningTime, (int)TEMPERATURE.AVERAGE, (int)CONTROL.RealDAC, (int)(CONTROL.AvgTimeOffset*1E9) );
			}
       	}
       	else {
       		SkipAgingTableCounter--;
       	}


		GainK = multiplyN(multiply(Pm0,transpose(HK)),(1/(MatToFloat(multiply(multiply(HK,Pm0),transpose(HK)))+RK)));
		HatX = Madd(HatXm0,(multiplyN(GainK,(CONTROL.AvgTimeOffset-MatToFloat(multiply(HK,HatXm0))))));
		P0 = multiply(Madd( IK, multiplyN(multiply(GainK,HK),-1.0)),Pm0);


        if ( SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS ){
			U.mat[1][0] = GPSPrevU;
	    } else {
	    	U.mat[1][0] = TRACE.MeanU;
	    }

		HatXm0 = Madd(multiply(PhiK,HatX),U);
		Pm0 = Madd( multiply(multiply(PhiK,P0),transpose(PhiK)),QK);

     	//#120814# sewon
        if( KInterval<Kinterval_STEP_2 )	CONTROL.TimeOffset = HatXm0.mat[1][0];
        else 					CONTROL.TimeOffset = CONTROL.AvgTimeOffset;

        //#120820# sewon
        if( IsCV_Slave() ){
        	CONTROL.TimeOffset = g_detail*1E-7;
        	dbgCV(1717)	printf("CONTROL.TimeOffset : %.2E\n",CONTROL.TimeOffset);
        }


        if ( SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS ){

         	GPSCurrentU = -(Kp*CONTROL.TimeOffset-(Kp-Ki*T)*CONTROL.PrevTimeOffset-GPSPrevU);
   		CONTROL.RealDAC = GPSCurrentU/((double)KGainF*(double)KInterval) + CONTROL.NominalDAC;

//	sprintf(MESG,"*DAC:%d U:%f N:%d*", (int)CONTROL.RealDAC, GPSCurrentU, (int)CONTROL.NominalDAC);
//	MessageOut(MESG);
			//##########################################################################################################
			//	DAC
			//	RealDAC ControlDAC
			//  ControlDAC
			//##########################################################################################################
			CheckControlDAC();
			average_rd_plus = average_rd_minus = 0;
			// CheckControlDAC
			// DAC Fail

//MessageOut("\r\nSetRbFreq2");
			//SetRbFreq();

			dbgCV(142) printf("[%.12lf, %.12lf, %.12lf, %05d]\n",CONTROL.TimeOffset,GPSCurrentU,CONTROL.RealDAC,CONFIG.DAC);
			RbSRO_Control();	//sewon 120503

   			TRACE.NominalDAC = CONTROL.PrevDAC = CONTROL.RealDAC;

			GPSPrevU = GPSCurrentU;
			TRACE.PrevAvgTimeOffset = TRACE.PrevU = 0.0;
        }
		//##############################################################################
		//##############################################################################



        // Frequency
		CONTROL.Frequency = (CONTROL.TimeOffset - CONTROL.PrevTimeOffset)/(double)KInterval;
		
		if( MAbs(CONTROL.Frequency) < 5E-10  && KInterval >= Kinterval_STEP_3 ){ freq_stable_cnt++; }
		else {freq_stable_cnt=0; }

		if(freq_stable_cnt >= 10) {freq_stable_enough = YES;}
		else {freq_stable_enough = NO;}


        CONTROL.PrevTimeOffset = CONTROL.TimeOffset;    // GPS Time offset


        // DACTableIndex
        index = DACTableIndex % MAX_DACTable_SIZE;
        DACTable[index] = CONTROL.RealDAC;  // HOLDOVER
        UTable[index] = GPSPrevU;  // KALMAN FILTER Scheduling

        DACTableIndex++;

   	    DACNo = (MAX_DACTable_SIZE > DACTableIndex ) ? DACTableIndex : MAX_DACTable_SIZE ;

   	    for( i = 0, DACsum = 0, Usum=0; i < DACNo  ; i++ ){
   	        DACsum += DACTable[i];
   	        Usum += UTable[i];
   	    }
        MeanDAC = DACsum / (double)DACNo;
        MeanU = Usum / (double)DACNo;


        dbgCV(3395)	printf("[%d, %d, %.3E]\n",CONTROL.StepCounter,KInterval,CONTROL.AvgTimeOffset);
        dbgCV(4848)	printf("[%.4lf, %.4lf]\n",MeanDAC,MeanU);


//sewon Stability Test~!//
//12-07-02 21:47//
////if( CONTROL.StepCounter == 100 ){
//       if((CONTROL.StepCounter >= 150) && (KInterval == Kinterval_STEP_1)){
//       		if(abs((int)(CONTROL.AvgTimeOffset*TENMILLION)) > 4){
//    			CONTROL.StepCounter = 1;
//    		}
//    		else{
//    			ChangeControlInterval(Kinterval_STEP_2);
//			}
//    	}
//    	//else if( CONTROL.StepCounter == 150 ){
//    	else if((CONTROL.StepCounter >= 180) && (KInterval == Kinterval_STEP_2)){
//			if(abs((int)(CONTROL.AvgTimeOffset*TENMILLION)) > 2){
//    			CONTROL.StepCounter = 150+1;
//    		}
//    		else{
//    			ChangeControlInterval(Kinterval_STEP_3);
//				CONTROL.CTRLMODE = CTRL_NORMAL;
//			}
//    	}
//    	else if((CONTROL.StepCounter >= 200) && (KInterval == Kinterval_STEP_3)){
//			if(abs((int)(CONTROL.AvgTimeOffset*TENMILLION)) > 1){
//    			CONTROL.StepCounter = 175+1;
//    		}
//    		else{
//    			ChangeControlInterval(Kinterval_STEP_4);
//				CONTROL.CTRLMODE = CTRL_NORMAL;
//			}
//    	}

		if((CONTROL.StepCounter >= Kinterval_STEP_Count_1) && (KInterval == Kinterval_STEP_1)){
			if(abs((int)(CONTROL.AvgTimeOffset*TENMILLION)) > 4){
				CONTROL.StepCounter = 1;
			}
			else{
				ChangeControlInterval(Kinterval_STEP_2);
			}
		}
		else if((CONTROL.StepCounter >= Kinterval_STEP_Count_2) && (KInterval == Kinterval_STEP_2)){
			if(abs((int)(CONTROL.AvgTimeOffset*TENMILLION)) > 2){
				CONTROL.StepCounter = Kinterval_STEP_Count_1 + 1;
			}
			else{
				ChangeControlInterval(Kinterval_STEP_3);
				CONTROL.CTRLMODE = CTRL_NORMAL;
			}
		}
		else if((CONTROL.StepCounter >= Kinterval_STEP_Count_3) && (KInterval == Kinterval_STEP_3)){
			if(abs((int)(CONTROL.AvgTimeOffset*TENMILLION)) > 1){
				CONTROL.StepCounter = Kinterval_STEP_Count_2 + 1;
			}
			else{
    			ChangeControlInterval(Kinterval_STEP_4);
    			CONTROL.CTRLMODE = CTRL_NORMAL;
			}
		}

#define LIMIT_OFFSET_QUICK_CONTROL_1	4
//#define LIMIT_OFFSET_QUICK_CONTROL_2	LIMIT_OFFSET_QUICK_CONTROL_1
#define LIMIT_OFFSET_QUICK_CONTROL_2	10

		if( KInterval == Kinterval_STEP_3 )
		{
			//if(abs((int)(CONTROL.AvgTimeOffset*TENMILLION)) > LIMIT_OFFSET_QUICK_CONTROL_1){
			//if( fabs(CONTROL.AvgTimeOffset) > 400e-9 && iControlRetryCnt++ >20 ){
			if( fabs(CONTROL.AvgTimeOffset) > 2000e-9 && iControlRetryCnt++ >20 ){
				ChangeControlInterval(Kinterval_STEP_3);
				//ChangeControlInterval(Kinterval_STEP_1);
				ControlCounter=0;
				freq_stable_cnt=0;
				freq_stable_enough = NO;
				CONTROL.SYNC = ON;
				iControlRetryCnt = 0;
				//bResetReceiver = 1;
//
				sprintf(MESG,">>Quick control, ro:%dns, avo:%dns, dac:%d, temp:%d",
					(int)(CONTROL.TimeOffset*BILLION), (int)(CONTROL.AvgTimeOffset*BILLION),
					(int)CONTROL.RealDAC,(int)TEMPERATURE.CURRENT
				);
	            TransmitMessage((BYTE*)MESG,DEBUG_PORT);
//				LogItString(LOG_CATA1_NORM, LOG_CATA2_WARN, MESG);
			}
		}

   		if (CONTROL.TimeOffset < LIMIT_OFFSET_QUICK_CONTROL_2 && CONTROL.TimeOffset > -LIMIT_OFFSET_QUICK_CONTROL_2 ){	//100ns
			LOCKEDtoGPS_SPEC_IN = OK;
       	} else {
			LOCKEDtoGPS_SPEC_IN = FAIL;
		}

        if ((SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS) && (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG)){
//            sprintf((char*)MESG,"%2d:%2d:%2d %3u> R:%7.2f AvgTraceOff:%12.11f AvgOff:%12.11f KOff:%15.14f TDAC:%5.2f Tnom:%5.2f Tmean:%5.2f \r\n",
//            sprintf((char*)MESG," AvgTraceOff:%.3E AvgOff:%.3E KOff:%.3E "
//            "TDAC:%5.2f Tnom:%5.2f Tmean:%5.2f,m,%.9le,r_o,%.9le,gCA,%.9le \r\n",
//                (float)TRACE.AvgTimeOffset,(float)CONTROL.AvgTimeOffset,(float)CONTROL.TimeOffset,
//                OCXO_ID.TDAC, Temper0, TEMPERATURE.AVERAGE, ModiValue, Recovery_Offset,gCurrentAll);
            sprintf((char*)MESG," AvgTraceOff:%.3E AvgOff:%.3E KOff:%.3E "
	            "TDAC:%5.2f Tnom:%5.2f Tmean:%5.2f,%d\r\n",
                (float)TRACE.AvgTimeOffset,(float)CONTROL.AvgTimeOffset,(float)CONTROL.TimeOffset,
                OCXO_ID.TDAC, Temper0, TEMPERATURE.AVERAGE, iControlRetryCnt);

            TransmitMessage((BYTE*)MESG,DEBUG_PORT);
        }
    }//KInterval(default Kinterval_STEP_3)

}







//################################################################
//  Master Control (LockedToGPS)
// 1. Control Stablizing Step
// 2. Recovery ġLocked
// 3. Switching Trace Master
//################################################################
void InitializeKalmanControl()
{


//	KInterval=Kinterval_STEP_3;
//    KNextInterval =Kinterval_STEP_4;

    PhiK.mat[0][0] = 0.36787944;
    PhiK.mat[0][1] = 0;
    PhiK.mat[0][2] = 0;
//
//    Kp = 0.1;
//    Ki = 0.05;
    if(!((VME.Data.TimeSync_Mode==VME_CV_MODE)&&(CONFIG.iCVType==1)))	Kp = 0.3;
    else	Kp = 0.8;
    Ki = 0.05;
    T = 0.1;
    RK = 10;

    QK.mat[0][0] = 800;
    QK.mat[1][1] = 10;

    //QK.mat[2][2] = 0.02;
    QK.mat[2][2] = 0.005; // Modifid [99-04-15 10:59]

    Pm0.mat[0][0] = 0;	Pm0.mat[0][1] = 0;	Pm0.mat[0][2] = 0;
	Pm0.mat[1][0] = 0;	Pm0.mat[1][1] = 0;	Pm0.mat[1][2] = 0;
	Pm0.mat[2][0] = 0;	Pm0.mat[2][1] = 0;	Pm0.mat[2][2] = 0;

	GainK.mat[0][0] = 0;
	GainK.mat[1][0] = 0;
	GainK.mat[2][0] = 0;

	HatX.mat[0][0] = 0;
	HatX.mat[1][0] = 0;
	HatX.mat[2][0] = 0;

	HatXm0.mat[0][0] = 0;
	HatXm0.mat[1][0] = 0;
	HatXm0.mat[2][0] = 0;

    //GPSPrevU=MeanU;
    GPSCurrentU=GPSPrevU=0.0;

	CONTROL.ControlDAC = MeanDAC;

	CONTROL.PrevDAC = MeanDAC;
	CONTROL.RealDAC = MeanDAC;
	CONTROL.NominalDAC = MeanDAC;

	CONTROL.TimeOffset = CONTROL.PrevTimeOffset = 0;
}


void InitializeKalmanControl_()
{

    PhiK.mat[0][0] = 0.36787944;
    PhiK.mat[0][1] = 0;
    PhiK.mat[0][2] = 0;

    Kp = 0.1;
    Ki = 0.05;
    T = 0.1;

    RK = 10;

    QK.mat[0][0] = 800;
    QK.mat[1][1] = 10;

    //QK.mat[2][2] = 0.02;
    QK.mat[2][2] = 0.005; // Modifid [99-04-15 10:59]


	Pm0.mat[0][0] = 0;	Pm0.mat[0][1] = 0;	Pm0.mat[0][2] = 0;
	Pm0.mat[1][0] = 0;	Pm0.mat[1][1] = 0;	Pm0.mat[1][2] = 0;
	Pm0.mat[2][0] = 0;	Pm0.mat[2][1] = 0;	Pm0.mat[2][2] = 0;

	GainK.mat[0][0] = 0;
	GainK.mat[1][0] = 0;
	GainK.mat[2][0] = 0;

	HatX.mat[0][0] = 0;
	HatX.mat[1][0] = 0;
	HatX.mat[2][0] = 0;

	HatXm0.mat[0][0] = 0;
	HatXm0.mat[1][0] = 0;
	HatXm0.mat[2][0] = 0;


	// DAC
    DACTableIndex = 0;

    //GPSPrevU=MeanU;
    GPSCurrentU=GPSPrevU=0.0;

	CONTROL.TimeOffset = CONTROL.PrevTimeOffset = 0;

//	CONTROL.ControlDAC=CONTROL.PrevDAC=CONTROL.RealDAC=CONTROL.NominalDAC=MeanDAC;

	CONTROL.NominalDAC=CONTROL.RealDAC;
	CONTROL.PrevDAC=CONTROL.RealDAC;
	CONTROL.ControlDAC=CONTROL.RealDAC;
}



void Wait_QuickStart()
{
	int i,index;
	float fsum;
	

//	if( OSC.kind == OCXO_OSC){
//	
//		MakePWMInterval((double)CONFIG.DAC);
//	  		CONTROL.CurrentDAC = (int)CONFIG.DAC;
//	
//		//kang ChangeDACValue((int)CONTROL.CurrentDAC);
//		ChangeDACValue(fdFPGA, (int)CONTROL.CurrentDAC);
//    }
//    else {
//
//		CONTROL.RealDAC = (int)CONFIG.DAC;
////MessageOut("\r\nSetRbFreq3");		
//		//SetRbFreq();
//
//	}
//
//
//	if( CGM_P() ){
//		if( IsX72Lock() && GC.RunningTime > WarmupTimeCounter ){
//			Init_QuickStart();
//			if(prev_running_time > 100){ QuickStartCounter = 90; }
//		}
//	}
//	else {
//
//		if( GC.RunningTime > WarmupTimeCounter ){
//
//			//if(SYSTEM.TIME.Valid){
//
//				if(CONTROL.warmup_counter >= WARMUP_READY_TIME){
//
//					Init_QuickStart();
//					if(prev_running_time > 100){ QuickStartCounter = 90; }
//				}
//
//				if(GC.RunningTime >= Module_Warmup_Time){
//					Init_QuickStart();
//					if(prev_running_time > 100){ QuickStartCounter = 90; }
//				}
//			//}
//		}
//	}
//
///*
//		if( GC.RunningTime > WarmupTimeCounter ){
//			Init_QuickStart();
//			if(prev_running_time > 100){ QuickStartCounter = 90; }
//		}
//*/
//
//	if(REFERENCE_MODE != PREV_REFERENCE_MODE){
//		CONTROL.warmup_counter = 0;
//		CONTROL.warmup_insertcounter = 0;
//		CONTROL.warmup_average = 0;	
//	}
//	
//	
//	if((REFERENCE_MODE == EB_STATUS_RUN)||(REFERENCE_MODE == LMU_STATUS_RUN)||(REFERENCE_MODE == UNDEFINED)){
//
//		CONTROL.warmup_insertcounter++;
//		index = CONTROL.warmup_insertcounter % WARMUP_AVERAGE_INTERVAL;
//		CONTROL.warmup_averagetable[index] = RAW_DIFFERENCE;
//		index = (CONTROL.warmup_insertcounter>WARMUP_AVERAGE_INTERVAL)?WARMUP_AVERAGE_INTERVAL:CONTROL.warmup_insertcounter;
//	    	    
//		for( i = 0, fsum = 0; i < index; i++ ) {
//			fsum += CONTROL.warmup_averagetable[i];
//		}
//		CONTROL.warmup_average = fsum/(float)index;
//	    
//	    
//		if( floatNaN(CONTROL.warmup_average)){
//			CONTROL.warmup_average = 9.9;
//		}		    
//		
//	}
//	else if(REFERENCE_MODE == E1_STATUS_RUN){
//		CONTROL.warmup_average = SYSTEM.E1.moving_average;
//		sprintf(MESG,"##e1-%.2f##",CONTROL.warmup_average);
//		MessageOut(MESG);
//	}
//	else if(REFERENCE_MODE == E2_STATUS_RUN){
//		CONTROL.warmup_average = SYSTEM.E2.moving_average;		
//		sprintf(MESG,"##e2-%.2f##",CONTROL.warmup_average);
//		MessageOut(MESG);
//	}
//		
//	if(CONTROL.warmup_average <= WARMUP_READY_RANGE && CONTROL.warmup_average >= -WARMUP_READY_RANGE) {
//		CONTROL.warmup_counter++;
//	}
//	else {
//		CONTROL.warmup_counter = 0;
//	}
//	
//	PREV_REFERENCE_MODE = REFERENCE_MODE;

	MakePWMInterval((double)CONFIG.DAC);
	ChangeDACValue(fdFPGA, (int)CONTROL.CurrentDAC);

	if(CONTROL.warmup_counter >= WARMUP_READY_TIME ||
		CONTROL.warmup_looks_good > 4){
		Init_QuickStart();
		LogItString(LOG_CATA1_NORM, LOG_CATA2_OK, "Ocxo heatup");
		//if(prev_running_time > 100){ QuickStartCounter = 90; }
	}

	if(GC.RunningTime >= MODULE_WARMUP_TIME_30MIN){
		Init_QuickStart();
		//if(prev_running_time > 100){ QuickStartCounter = 90; }
		LogItString(LOG_CATA1_NORM, LOG_CATA2_FAIL, "Ocxo heatup");
	}
	
    index = CONTROL.warmup_insertcounter % WARMUP_AVERAGE_INTERVAL;
	CONTROL.warmup_insertcounter++;
		
	CONTROL.warmup_averagetable[index] = RAW_DIFFERENCE;
	index = (CONTROL.warmup_insertcounter>WARMUP_AVERAGE_INTERVAL)?WARMUP_AVERAGE_INTERVAL:CONTROL.warmup_insertcounter;
	    	    
	for( i = 0, fsum = 0; i < index; i++ ) {
		fsum += CONTROL.warmup_averagetable[i];
	}
	CONTROL.warmup_average = fsum/(double)index;
		
    if(CONTROL.warmup_insertcounter%WARMUP_AVERAGE_INTERVAL ==0){
	    	
    	index = CONTROL.warmup_avgavgcounter % WARMUP_AVERAGE_INTERVAL;
		CONTROL.warmup_avgavgcounter++;
	
    	CONTROL.warmup_avgavgtable[index] = CONTROL.warmup_average;
    	index = (CONTROL.warmup_avgavgcounter>WARMUP_AVERAGE_INTERVAL)?WARMUP_AVERAGE_INTERVAL:CONTROL.warmup_avgavgcounter;
    	
    	CONTROL.warmup_avgmin = 999999999.0;
    	CONTROL.warmup_avgmax = -999999999.0;
    	
    	for( i = 0; i < index; i++ ) {
    		if(CONTROL.warmup_avgavgtable[i]<CONTROL.warmup_avgmin){
    			CONTROL.warmup_avgmin = CONTROL.warmup_avgavgtable[i];
    		}
    		if(CONTROL.warmup_avgavgtable[i]>CONTROL.warmup_avgmax){
    			CONTROL.warmup_avgmax = CONTROL.warmup_avgavgtable[i];
    		}
		}
		
		if(CONTROL.warmup_avgavgcounter>=WARMUP_AVERAGE_INTERVAL){
    		if((fabs(CONTROL.warmup_avgmin-CONTROL.warmup_avgmax)<0.2) &&
    			(fabs(CONTROL.warmup_avgmax)<3.0)){
    				CONTROL.warmup_looks_good++;
    		}
    		else{CONTROL.warmup_looks_good = 0;}
    	}
    	else{
    		CONTROL.warmup_avgmin = 0.0;
    		CONTROL.warmup_avgmax = 0.0;
    	}
	}
	
	if(isnan(CONTROL.warmup_average)){
		CONTROL.warmup_average = 9.9;
	}		    
		
	if((CONTROL.warmup_average <= WARMUP_READY_RANGE && CONTROL.warmup_average >= -WARMUP_READY_RANGE) &&
		(SYSTEM.TRACK.TRACKED>2)) {
		//if(CONTROL.warmup_counter == 0){
		//	printf("warming up(in): avg-rd: %3.2f, min/max: %3.2f/%3.2f(%d)\n",
		//		(float)CONTROL.warmup_average, (float)CONTROL.warmup_avgmin, (float)CONTROL.warmup_avgmax,
		//		CONTROL.warmup_looks_good
		//	);
		//}
		
		CONTROL.warmup_counter++;
	}
	else {
		//if(CONTROL.warmup_counter != 0){
		//	printf("warming up(out): avg-rd: %3.2f, min/max: %3.2f/%3.2f(%d)\n",
		//		(float)CONTROL.warmup_average, (float)CONTROL.warmup_avgmin, (float)CONTROL.warmup_avgmax,
		//		CONTROL.warmup_looks_good
		//	);
		//}
		CONTROL.warmup_counter = 0;
	}
	
    if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
		//sprintf((char*)MESG,"<%.3d>%2d:%2d:%2d :"
		//					"RO:%5.2f RD:%5.2f OT:%5.2f C:%5.2f,"
		//					"m,%.2lf,r_o,%.2lf,gCA,%.2lf,"
		//					"[PS,%d,RM,%d,pRM,%d,mwt,%d][%.1f,%d]\r\n",
		//	WarmupTimeCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
		//	RAW_OFFSET,RAW_DIFFERENCE,TEMPERATURE.CURRENT,TEMPERATURE.current,
		//	ModiValue,Recovery_Offset,gCurrentAll,
		//	CGM_P(),REFERENCE_MODE,PREV_REFERENCE_MODE,Module_Warmup_Time,CONTROL.warmup_average,CONTROL.warmup_counter);
		sprintf((char*)MESG,"%2d:%2d:%2d :"
							" RO:%5.2f RD:%5.2f OT:%5.2f"
							//" gCA: %.2lf"
							" WO:%d"
							" Pri:%d,%d [%.2f,%d][%.2f,%.2f,%d][%d]\r\n",
			SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
			RAW_OFFSET,RAW_DIFFERENCE,TEMPERATURE.CURRENT,
			//gCurrentAll,
			(int)(warmup_toffset),
			psda.my_priority_value, psda.your_priority_value,
			CONTROL.warmup_average,CONTROL.warmup_counter,
			CONTROL.warmup_avgmin, CONTROL.warmup_avgmax, CONTROL.warmup_looks_good,
			IsSigOn());
		TransmitMessage((BYTE*)MESG,DEBUG_PORT);
	}

}

void Init_QuickStart()
{

	SYSTEM.OPERATION.OPMODE = OPMODE_WARMUP;
	CONTROL.SURVEYSTEP = Survey_InitializePLL;
    CONTROL.IntervalCounter = 0;
    CONTROL.CTRLMODE = CTRL_OCXO_HEATUP_READY;

	CONTROL.StepCounter = 0;

    QuickStartCounter = 0;

}


void QuickStartControl(char control_turn)
{
	int t;
	QuickStartCounter++;

   	if(CounterModule.STATUS == STATUS_ACTIVE){

		MyModule.StatusEvent = BLOCK_TO_ACTIVE_STANDBY;
		//CONTROL.CTRLMODE = CTRL_QUICKSTART;
		FIRST_LOCK = ON;
		MessageOut("<5:B2AS>");
		//LogIt3(LOG_SWITCH_B2AS,GetParameter1(),GetParameter2(),GetParameter3());
		LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, LOG_SWITCH_B2AS,GetParameter1(),GetParameter2(),223);
//printf("********+++++BLOCK_TO_ACTIVE_STANDBY 1111********+++++\n");
	}


	if(TEMP_RAW_DIGITS < -10 || TEMP_RAW_DIGITS > 10){
		CONTROL.StepCounter = 0;
		if(QuickStartCounter == 220){
			if(warmup_stable_retry<10){QuickStartCounter = 100;}
			if(warmup_stable_retry > 4 && warmup_stable_retry < 8){
				control_sync_type = 10;
				CONTROL.SYNC = ON;
				SKIP_CALC = ON;
			}
			
			warmup_stable_retry++;	
			
		}
	}		
			
	//if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
	//	sprintf(MESG," QI:%d %d ",QuickStartCounter, warmup_stable_retry);
	//	TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	//}
			
	//if( control_turn == holdover_control ){
    //
	//	if( QuickStartCounter >= 10 ){
    //
	//		if(SYSTEM.OPERATION.STATUS != STATUS_ACTIVE){
	//			MyModule.StatusEvent = BLOCK_TO_ACTIVE_STANDBY;
	//			MessageOut("<2:B2AS>");
	//			LogIt3(LOG_SWITCH_B2AS,GetParameter1(),GetParameter2(),GetParameter3());
	//		}
	//       	//LogIt(LOG_Lock,0);
	//		//CURR_SIG_STATE = ON;
    //
	//		CONTROL.CTRLMODE = CTRL_QUICKSTART;
	//		FIRST_LOCK = ON;
	//	}
	//}
	//else {

		//if( QuickStartCounter == 110 ){
		//	//EVENT.PLL = ON;
		//	//MessageOut("PLL Start!!\r\n");
		//}
		//else if( QuickStartCounter == 125 ){
		//	//CONTROL.EVEN_SYNC = ON;
		//	//MessageOut("Even Sec. Sync.!!\r\n");
		//}
		
		//if( QuickStartCounter == 130 ){
		if( QuickStartCounter == 220 ){

			if(SYSTEM.OPERATION.STATUS != STATUS_ACTIVE){
				MyModule.StatusEvent = BLOCK_TO_ACTIVE_STANDBY;
				MessageOut("<1:B2AS>");
				//LogIt3(LOG_SWITCH_B2AS,GetParameter1(),GetParameter2(),GetParameter3());
				LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, LOG_SWITCH_B2AS,GetParameter1(),GetParameter2(),123);
//printf("********+++++BLOCK_TO_ACTIVE_STANDBY 2222********+++++\n");
			}
	       	//LogIt(LOG_Lock,0);
			//CURR_SIG_STATE = ON;

			//CONTROL.CTRLMODE = CTRL_QUICKSTART;
			FIRST_LOCK = ON;
		}
	//}

}


void End_QuickStart()
{

}




void ChangeControlInterval(int interval)
{
	char buf[30];

//	if(interval>Kinterval_STEP_1)	return;

	switch(interval){

		case Kinterval_STEP_1:
	       		InitializeKalmanControl();
	       		KInterval = Kinterval_STEP_1;    KNextInterval = Kinterval_STEP_1;
    	   		SKIP_CONTROL = ON;
    	   		CONTROL.StepCounter = 0;
				break;

		case Kinterval_STEP_2:
	       		InitializeKalmanControl();
        	    QK.mat[0][0] = 800;
            	QK.mat[1][1] = 10;
	            QK.mat[2][2] = 0.15;
//				Kp = 0.3;
//        	    Ki = 0.05;
//            	T = 0.1;
	       		KInterval = Kinterval_STEP_2;    KNextInterval = Kinterval_STEP_2;
    	   		SKIP_CONTROL = ON;
				break;

		case Kinterval_STEP_3:
	    		InitializeKalmanControl();
        	    //QK.mat[0][0] = 1800;
	    		QK.mat[0][0] = 800;
            	QK.mat[1][1] = 10;
	            QK.mat[2][2] = 0.05;
//				Kp = 0.3;
//				Ki = 0.05;
//            	T = 0.1;
	   			KInterval = Kinterval_STEP_3;    KNextInterval = Kinterval_STEP_3;
   				SKIP_CONTROL = ON;
				break;

		case Kinterval_STEP_4:
    			InitializeKalmanControl();
    	        QK.mat[0][0] = 800;
        	    QK.mat[1][1] = 10;
	            QK.mat[2][2] = 0.005;
//				Kp = 0.3;
//				Ki = 0.05;
//            	T = 0.1;
  				KInterval = Kinterval_STEP_4;    KNextInterval = Kinterval_STEP_4;
  				SKIP_CONTROL = ON;
				break;
	}
}

void Update_ModuleWarmupTime(float temper)
{
	if(temper<WARMUP_TIME_BOUNDARY){
		Module_Warmup_Time = MODULE_WARMUP_TIME_30MIN;
	}
	else{
		Module_Warmup_Time = MODULE_WARMUP_TIME_10MIN;
	}
	sprintf(MESG,"###Update MWT,%d,temp,%.2f,B,%d\r\n",
					Module_Warmup_Time,	temper,(int)WARMUP_TIME_BOUNDARY
	);
	MessageOut(MESG);
}


void ffInitValue(FreqFilter *pFF)
{
    int i;
	pFF->Index =0;
	pFF->IndexAve =0;
	pFF->cntData = 0;
	pFF->dROsum =0;
	for( i=0; i<aveLimit; i++ ){
		pFF->dRO[i] =0;
		pFF->AvedRO [i]=0;
	}
}

void ffInit(FreqFilter *pFF)
{
	pFF->turn =0;
	pFF->once =0;
	pFF->diffFreq=0;
	pFF->freqFailCnt=0;
	ffInitValue(pFF);
}

void ffRefInit(_aveRO *pRef)
{
	pRef->sumDAC =0;
	pRef->sumTEMP =0;
	pRef->centerDAC=0;
	pRef->centerTEMP=0;
	pRef->cntGood=0;
}


//#ifndef fabs(x)
//#define fabs(x)	((x)>0?(x):-(x))  //tgmath.h
//#endif
 
int ffPush(int Lock, double RO, double DAC,double TEMP)
{
	FreqFilter	*pFF;
	_aveRO	*pRef;
	double freq = 0;
	double aveROtmp1,aveROtmp2;
	double diffFreq;
	double dTmp;
	int 	bCalAvail=0;

	pFF = &ffD;
	//if( bFirstFreqFilter ){
	if( bFirstFreqFilter || bResetReceiver){

		bResetReceiver = 0;
		ffInit(pFF);
		bFirstFreqFilter =0;
		pFF->ReceiverGood=1;
		return FF_FIRST;
	}

	//if( !Lock || bResetReceiver){
	if( !Lock ){
		//bResetReceiver = 0;
		ffInitValue(pFF);
		pFF->ReceiverGood=1;
		return FF_UNLOCK;
	}

	pFF->dROsum	+= RO;

	//pFF->dDAC[ pFF->Index ] +=DAC;
	pFF->dRO[  pFF->Index ] =RO;

	pFF->Index++;
	pFF->Index %= aveLimit;

	if( pFF->cntData >= aveLimit ){
		
		
		pFF->dROsum 	-= pFF->dRO[  pFF->Index];

		//1. sampling aveRO
		//pRef->AvedDAC[ pRef->IndexAve ] = pRef->dDACsum/aveLimit;
		pFF->AvedRO [ pFF->IndexAve ] = pFF->dROsum /aveLimit; // moving average



		aveROtmp1 = pFF->AvedRO [ pFF->IndexAve ];

		pFF->IndexAve++;
		pFF->IndexAve %= aveLimit; 


		aveROtmp2 = pFF->AvedRO [ pFF->IndexAve ];
		//pFF->cntAve++;
		freq = ( aveROtmp1 - aveROtmp2 ) / (double)aveLimit ;
		freq *=1E-7;
		pFF->Freq=freq;

		if(pFF->cntData > 10*60) {bCalAvail =1;}
		pFF-> bCalAvail =  bCalAvail;
	}

	pFF->cntData++;


	pRef = & ( pFF->Ref[ pFF->turn ] );
	if( bCalAvail && fabs(freq) < freqGoodLimit ){
		pRef->sumDAC +=DAC;
		pRef->sumTEMP +=TEMP;
		pRef->cntGood++;
		if( pRef->cntGood > aveLimit ){
			pRef->centerDAC = pRef->sumDAC/(double)pRef->cntGood;
			pRef->centerTEMP = pRef->sumTEMP/(double)pRef->cntGood;
			pFF->turn = !(pFF->turn);

			pFF->once = 1;
		
			pRef = & ( pFF->Ref[ ffD.turn ] );
			ffRefInit(pRef);
		}
	}
	else{
		ffRefInit(pRef);
	}
	
	pref_centerdac = pRef->centerDAC;

	pRef = & ( pFF->Ref[ !pFF->turn ] );
	pFF->bCalAvail=bCalAvail;
	diffFreq = freq - (DAC-pRef->centerDAC)*DACGain;
	pFF->diffFreq = diffFreq ;
	
	//if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
	//	sprintf(MESG," A:%d O:%d FQ:%+5.3E D:%+5.3E C:%7.2f T:%5.2f ",
	//		bCalAvail, pFF->once, 
	//		pFF->Freq,
	//		diffFreq,
	//		pRef->centerDAC,
	//		pRef->centerTEMP
	//		);
	//	TransmitMessage((BYTE*)MESG,PORT_CONTROL);
	//}
//	if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
//		sprintf(MESG," A:%d O:%d FQ:%+5.3E D:%+5.3E C:%7.2f ",
//			bCalAvail, pFF->once, 
//			pFF->Freq,
//			diffFreq,
//			pRef->centerDAC
//			);
//		TransmitMessage((BYTE*)MESG,PORT_CONTROL);
//	}

	if( bCalAvail && pFF->once && fabs(pRef->centerTEMP - TEMP)< 30 ){
		
		dTmp= (DAC-pRef->centerDAC)*DACGain;
		if(  fabs(diffFreq)> freqFailLimit ){
			pFF->ReceiverGood=0;

			//ffInitValue(pFF);
			ffInit(pFF);

			if( ++pFF->freqFailCnt > ffRestCriteria ){
				bFirstFreqFilter=1;
			}
			return FF_UNSTABLE;
		}
		else{
			return FF_STABLE;
		}
	}
	pFF->ReceiverGood=1;
	return FF_OTHERS;
}


